{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "5e24589a-461d-46b5-8141-37f948dcf4dc",
   "metadata": {},
   "source": [
    "# cutting tool for a worm gear\n",
    "\n",
    "1. idea 1:  \n",
    "\n",
    "<img src=\"../../docs/computing a profile_from_a_given_rack.jpg\">"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "7eacf041-aa83-49e2-9cbe-066f177197f6",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sym\n",
    "import numpy as np\n",
    "from pygears.transformation import symbolic_transformation, numeric_transformation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "5852aa56-e66b-4f3c-a50d-0cb4cb21abd2",
   "metadata": {},
   "outputs": [],
   "source": [
    "t = sym.Symbol(\"t\")\n",
    "T1 = symbolic_transformation(np.pi / 2.,\n",
    "                             np.array([1., 0., 0.]),\n",
    "                             np.array([12.5,0., 1.15]))\n",
    "T2 = symbolic_transformation(-t / 7.5,\n",
    "                             np.array([0., 0., 1.]),\n",
    "                             np.array([0., 0., 0.]))\n",
    "T3 = symbolic_transformation(0.,\n",
    "                             np.array([1., 0., 0.]),\n",
    "                             np.array([0., 0., t]))\n",
    "\n",
    "T = sym.nsimplify(T2.inv() @ T1.inv() @ T3, tolerance=10e-16)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "id": "7c9837b8-caf7-4447-bf0d-eba9085197a5",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[ 0.        ,  0.        , -0.13333333,  0.15333333],\n",
       "       [-0.13333333,  0.        ,  0.        ,  0.66666667],\n",
       "       [ 0.        ,  0.        ,  0.        ,  0.        ],\n",
       "       [ 0.        ,  0.        ,  0.        ,  0.        ]])"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T_fn = sym.lambdify(t, T)\n",
    "dT_fn = sym.lambdify(t, T.diff(t))\n",
    "dT_fn(0.)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e4354a36-409a-40a6-8f4f-bb5a6c84f3b7",
   "metadata": {},
   "source": [
    "Diese Methode funktioniert nicht, weil die Bedingung zur Bestimmung des Kontaktpunktes falsch ist.\n",
    "Eine Bedingung für die generierung einer konstanten Übersetzung ist, dass die Normale auf die Kontaktfläche (Zahnstange) immer durch den Punkt p (Eingriffspunkt für Ersatzzahnrad (Zylinder) und Ersatzzahnstange (Quader) gehen muss.\n",
    "Gesucht sind also Punkte auf der Fläche S welche verbunden mit P normal auf die Fläche stehen. Dies kann auch als minimaler Abstand von P zur Fläche gesehen werden.\n",
    "\n",
    "für jedes u: min(norm(S(u,v)-P)) -> d(norm(S(u,v)-P)/dv = 0\n",
    "die Änderung des Abstands ist 0 -> die Gerade steht normal auf die Fläche\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "43f00b65-c05c-4734-8c5a-e7845a6f4dff",
   "metadata": {},
   "source": [
    "## Vorgehensweise\n",
    "\n",
    "1. approximate the surface by a BSplineSurface\n",
    "\n",
    "- Erstellen eines \"Cross-Sektion\" objekts aus dem \"Werkzeug\"\n",
    "\n",
    "<img src=\"cross_section.png\">  \n",
    "\n",
    "- Draft downgrade um Kanten zu bekommen\n",
    "das Cross-section Objekt beinhaltet nicht die anfangs und end Kanten. Diese müssen zusätzlich vom Werkzeug extrahiert werden\n",
    "\n",
    "- Part Loft zum erstellen einer schönen BSplinefläche\n",
    "\n",
    "<img src=\"loft_bspline_flaechen.png\"> \n",
    "\n",
    "3. Loft -> Surface\n",
    "\n",
    "```python\n",
    "# select the cutting faces\n",
    "face_1 = App.ActiveDocument.Loft.Shape.Faces[0].copy()\n",
    "face_2 = App.ActiveDocument.Loft001.Shape.Faces[0].copy()\n",
    "\n",
    "# compute the contact curve:\n",
    "bsp_1 = face_1.Surface\n",
    "bsp_2 = face_2.Surface\n",
    "\n",
    "```\n",
    "\n",
    "4. Minimierung des Abstands zum \"Pitch-Punkt\"\n",
    "\n",
    "```python\n",
    "import scipy as scp\n",
    "point = App.Vector(5., 0., 1.15 - time) \n",
    "xyz_1 = []\n",
    "for v in np.linspace(0, 1, 5):\n",
    "        def dist_1(u):\n",
    "            distance = bsp_1.value(u, v) - point\n",
    "            return distance.x ** 2 + distance.z ** 2\n",
    "        u_1 = scp.optimize.minimize(dist_1, 0.5, tol=1e-6).x[0]\n",
    "        xyz_1.append(bsp_1.value(u_1, v))\n",
    "```\n",
    "\n",
    "5. erstellen einer B-Spline Kurve welche durch die Kinematik T transformiert wird\n",
    "\n",
    "```python\n",
    "c_1 = part.BSplineCurve()\n",
    "c_1.interpolate(Points=xyz_1)\n",
    "c_1 = c_1.toShape()\n",
    "\n",
    "part.show(c_1.transformShape(T))\n",
    "```\n",
    "\n",
    "6. Loft anwenden auf die erstellten BSpline Kurven\n",
    "\n",
    "<img src=\"loft_of_generated_bsplines.png\">\n",
    "\n",
    "7. Array für das Zahnrad\n",
    "\n",
    "<img src=\"gear_assembly.png\">"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "id": "cfd8026b-5a84-4882-a1de-63580776a579",
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sym\n",
    "import numpy as np\n",
    "import scipy as sp\n",
    "\n",
    "from pygears.transformation import symbolic_transformation, numeric_transformation\n",
    "\n",
    "t, x, z, m, r_w = sym.symbols([\"t\", \"x\", \"z\", \"m\", \"r_w\"], real=True)\n",
    "s, alpha, n_t, y, phi = sym.symbols([\"s\", \"alpha\", \"n_t\", \"y\", \"phi\"], real=True, positiv=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "618dd2ae-e8e5-430d-8c8c-8983358334b7",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & 0 & r_{w}\\\\0 & 1 & 0 & 0\\\\0 & 0 & 1 & 0\\\\0.0 & 0.0 & 0.0 & 1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[  1,   0,   0, r_w],\n",
       "[  0,   1,   0,   0],\n",
       "[  0,   0,   1,   0],\n",
       "[0.0, 0.0, 0.0, 1.0]])"
      ]
     },
     "execution_count": 9,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "p0 = symbolic_transformation(0,np.array([0, 0, 1]), [r_w, 0, 0, 1])\n",
    "p0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "0f305b8b-0fb5-4b71-80b6-9a2b43b59e26",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}r_{w} + s \\cos{\\left(\\alpha \\right)}\\\\0\\\\- s \\sin{\\left(\\alpha \\right)}\\\\1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[r_w + s*cos(alpha)],\n",
       "[                 0],\n",
       "[     -s*sin(alpha)],\n",
       "[               1.0]])"
      ]
     },
     "execution_count": 11,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "l = p0 @ sym.Matrix([s * sym.cos(alpha), 0, -s * sym.sin(alpha), 1])\n",
    "l"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "65bb90d7-0f5b-410e-9a3a-0f9953a4d846",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\phi \\right)} & \\sin{\\left(\\phi \\right)} & 0 & 0\\\\- \\sin{\\left(\\phi \\right)} & \\cos{\\left(\\phi \\right)} & 0 & 0\\\\0 & 0 & 1 & m n_{t} \\phi\\\\0.0 & 0.0 & 0.0 & 1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ cos(phi), sin(phi),   0,         0],\n",
       "[-sin(phi), cos(phi),   0,         0],\n",
       "[        0,        0,   1, m*n_t*phi],\n",
       "[      0.0,      0.0, 0.0,       1.0]])"
      ]
     },
     "execution_count": 19,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "T_spiral = symbolic_transformation(phi, np.array([0, 0, 1]), np.array([0, 0, m * phi * n_t]))\n",
    "T_spiral"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "da3c8575-99ad-4258-8734-c165ea65b014",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\left(r_{w} + s \\cos{\\left(\\alpha \\right)}\\right) \\cos{\\left(\\phi \\right)}\\\\- \\left(r_{w} + s \\cos{\\left(\\alpha \\right)}\\right) \\sin{\\left(\\phi \\right)}\\\\1.0 m n_{t} \\phi - s \\sin{\\left(\\alpha \\right)}\\\\1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[ (r_w + s*cos(alpha))*cos(phi)],\n",
       "[-(r_w + s*cos(alpha))*sin(phi)],\n",
       "[  1.0*m*n_t*phi - s*sin(alpha)],\n",
       "[                           1.0]])"
      ]
     },
     "execution_count": 20,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "spiral = (T_spiral @ l)\n",
    "spiral"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "id": "e47eb83b-6e89-4246-a82a-bd5629aedc2a",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\operatorname{acos}{\\left(\\frac{x}{r_{w} + s \\cos{\\left(\\alpha \\right)}} \\right)}$"
      ],
      "text/plain": [
       "acos(x/(r_w + s*cos(alpha)))"
      ]
     },
     "execution_count": 21,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "x_cross_section = sym.simplify(sym.solve(spiral[0] - x, phi)[1])\n",
    "x_cross_section"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "id": "c2954b39-eea0-4e27-987f-07a5d0dedaad",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}x\\\\- \\sqrt{- \\frac{x^{2} - \\left(r_{w} + s \\cos{\\left(\\alpha \\right)}\\right)^{2}}{\\left(r_{w} + s \\cos{\\left(\\alpha \\right)}\\right)^{2}}} \\left(r_{w} + s \\cos{\\left(\\alpha \\right)}\\right)\\\\1.0 m n_{t} \\operatorname{acos}{\\left(\\frac{x}{r_{w} + s \\cos{\\left(\\alpha \\right)}} \\right)} - s \\sin{\\left(\\alpha \\right)}\\\\1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[                                                                                    x],\n",
       "[-sqrt(-(x**2 - (r_w + s*cos(alpha))**2)/(r_w + s*cos(alpha))**2)*(r_w + s*cos(alpha))],\n",
       "[                                1.0*m*n_t*acos(x/(r_w + s*cos(alpha))) - s*sin(alpha)],\n",
       "[                                                                                  1.0]])"
      ]
     },
     "execution_count": 22,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "spiral_x = sym.simplify(spiral.subs({phi: x_cross_section}))\n",
    "spiral_x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "id": "268c6302-4e7d-45e0-be28-1b64cf8b766e",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{- r_{w} + \\sqrt{x^{2} + y^{2}}}{\\cos{\\left(\\alpha \\right)}}$"
      ],
      "text/plain": [
       "(-r_w + sqrt(x**2 + y**2))/cos(alpha)"
      ]
     },
     "execution_count": 29,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "y_cross_section = sym.simplify(sym.solve(spiral_x[1]- y, s)[0])\n",
    "y_cross_section\n",
    "y_cross_section = (sym.sqrt(x**2 + y**2) - r_w) / sym.cos(alpha)\n",
    "y_cross_section"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "id": "7284bd6a-b47b-483c-8e9f-79fcaecce5e3",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}x\\\\- y\\\\1.0 m n_{t} \\operatorname{acos}{\\left(\\frac{x}{r{\\left(x,y \\right)}} \\right)} + 1.0 r_{w} \\tan{\\left(\\alpha \\right)} - 1.0 r{\\left(x,y \\right)} \\tan{\\left(\\alpha \\right)}\\\\1.0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[                                                                      x],\n",
       "[                                                                     -y],\n",
       "[1.0*m*n_t*acos(x/r(x, y)) + 1.0*r_w*tan(alpha) - 1.0*r(x, y)*tan(alpha)],\n",
       "[                                                                    1.0]])"
      ]
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "f_r = sym.Function(\"r\")(x, y)\n",
    "spiral_xy = sym.simplify(spiral_x.subs({s: y_cross_section}))\n",
    "spiral_xy = spiral_xy.subs({sym.Abs(y): y, sym.sqrt(x**2 + y**2): f_r})\n",
    "spiral_xy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "id": "e7b9f7cc-59f0-4dc6-922c-c94a753c0fd5",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle z{\\left(x,y \\right)}$"
      ],
      "text/plain": [
       "z(x, y)"
      ]
     },
     "execution_count": 31,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "z = sym.Function(\"z\")(x, y)\n",
    "z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "id": "080e1845-8ff4-493e-aa28-5b677bfa3cb4",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{y - y_{p} + z{\\left(x,y \\right)} \\frac{\\partial}{\\partial y} z{\\left(x,y \\right)}}{\\sqrt{\\left(y - y_{p}\\right)^{2} + z^{2}{\\left(x,y \\right)}}}$"
      ],
      "text/plain": [
       "(y - y_p + z(x, y)*Derivative(z(x, y), y))/sqrt((y - y_p)**2 + z(x, y)**2)"
      ]
     },
     "execution_count": 32,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "y_p =sym.Symbol(\"y_p\")\n",
    "dist_p = sym.sqrt(z**2 + (y - y_p)**2)\n",
    "dist_p.diff(y)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d9559950-1520-439c-8e7a-35b4006f104a",
   "metadata": {},
   "source": [
    "## for x = 0, for a first guess of t"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "0811a6d6-4544-4a99-b4bd-ba5cf6b9027e",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "PATH_TO_FREECAD_LIBDIR not specified, using default FreeCAD version in /Users/lo/mambaforge/envs/freecad/lib\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "<Part::PartFeature>"
      ]
     },
     "execution_count": 1,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import scipy as sp\n",
    "import numpy as np\n",
    "from freecad import part\n",
    "from freecad import app\n",
    "from pygears.transformation import numeric_transformation\n",
    "\n",
    "debug = False\n",
    "def compute_involute(module=1, teeth=15, height=5, worm_pitch_diameter=10, num_threads=1, alpha=np.deg2rad(20)):\n",
    "    x = 0.\n",
    "    r_w = module * teeth / 2\n",
    "    x_p = worm_pitch_diameter / 2\n",
    "    r_thales = r_w / 2.\n",
    "    x_thales = y_p + r_thales\n",
    "    \n",
    "    def length(y):\n",
    "        return (x**2 + y**2)**(0.5)\n",
    "    \n",
    "    def dlength_dy(y):\n",
    "        return y / length(y)\n",
    "    \n",
    "    def z(y, t):\n",
    "        r = length(y)\n",
    "        return - module * num_threads * np.arcsin(x / r) / 2 + (r-r_w) * np.tan(alpha) + t\n",
    "    \n",
    "    def dz_dy(y):\n",
    "        r = length(y)\n",
    "        return module * num_threads * x * dlength_dy(y) / \\\n",
    "               (2 * np.sqrt(1 - x ** 2 / r ** 2 ) * r ** 2) + \\\n",
    "               np.tan(alpha) * dlength_dy(y)\n",
    "        \n",
    "    def distance_yp(y, t):\n",
    "        return np.sqrt((y_p - y) ** 2 + z(y, t) ** 2)\n",
    "\n",
    "    def distance_yp_2(y, t):\n",
    "        return (y_p - y) ** 2 + z(y, t) ** 2\n",
    "    \n",
    "    def ddistance_yp_dy(y, t):\n",
    "        return (y - y_p + z(y, t) * dz_dy(y)) / distance_yp(y, t)\n",
    "    \n",
    "    def distance_ythales(y, t):\n",
    "        return np.sqrt((y_thales - y) ** 2 + z(y, t) ** 2)\n",
    "    \n",
    "    def min_ground(pars):\n",
    "        y, t = pars\n",
    "        # return the normal-condition + the intesection of the tooth-flank with the thales circle\n",
    "        return ddistance_yp_dy(y, t) ** 2 + (distance_ythales(y, t) - r_thales) ** 2\n",
    "\n",
    "    def min_head(pars):\n",
    "        y, t = pars\n",
    "        r_0 = y_p -  5 * module # * (1 + clearence)\n",
    "        # y_inner = r_0 * np.cos(np.arcsin(x / r_0))\n",
    "        return ddistance_yp_dy(y, t) ** 2 + (y - r_0) ** 2\n",
    "\n",
    "    def analytic_solution_for_x_0():\n",
    "        t = - (r_w + y_p) * np.tan(alpha)\n",
    "        y = y_p + r_w * np.sin(alpha) ** 2\n",
    "        return np.array([y, t])\n",
    "    \n",
    "    start = analytic_solution_for_x_0()\n",
    "    if debug:\n",
    "        print(f\"analytic solution:   {analytic_solution_for_x_0()}\")\n",
    "        print(f\"min_ground analytic: {min_ground(start)}\")\n",
    "        print(f\"thales analytic:     {distance_ythales(start[0], start[1]) - r_thales}\")\n",
    "        print(f\"normal analytic:     {ddistance_yp_dy(start[0], start[1])}\")\n",
    "        print()\n",
    "\n",
    "    # t_end is once computed for x=0\n",
    "    t_end = sp.optimize.minimize(min_head, [y_p, 0.]).x[1]\n",
    "\n",
    "    xyz = []\n",
    "    for x in np.linspace(-height / 2, height / 2, 20):\n",
    "        xyz_section = []\n",
    "        t_start = sp.optimize.minimize(min_ground, start).x[1]\n",
    "        for t in np.linspace(t_start, t_end, 20):\n",
    "\n",
    "            # compute the time (t) dependent transformation\n",
    "            phi = np.pi / 2\n",
    "            phi += y_p * np.tan(alpha) / r_w\n",
    "            phi += - np.sign(alpha) * module * np.pi / 4. / r_w\n",
    "            phi += t / r_w\n",
    "            T_0 = numeric_transformation(phi, np.array([0., 0., 1.]))\n",
    "            T_1 = numeric_transformation(-np.pi / 2, np.array([0., 1., 0.]))\n",
    "            T_2 = numeric_transformation(0., np.array([0., 0., 1.]), np.array([0, y_p + r_w, 0.]))\n",
    "            T = np.linalg.inv(T_2 @ T_1 @ T_0)\n",
    "            \n",
    "            # find point on curve for given t\n",
    "            y = sp.optimize.root(ddistance_yp_dy, y_p, (t)).x[0]\n",
    "            z_i = z(y, t) #  - y_p * np.tan(alpha) + np.sign(alpha) * module * np.pi / 4\n",
    "            point = np.array([x, y, z_i, 1.])\n",
    "            xyz_section.append((T @ point)[:3])\n",
    "        xyz.append(np.array(xyz_section))\n",
    "\n",
    "    return np.array(xyz)\n",
    "\n",
    "# parameters\n",
    "module = 1.\n",
    "teeth = 50\n",
    "height = 5\n",
    "worm_pitch_diameter = 10\n",
    "num_threads = 1\n",
    "alpha = np.deg2rad(20)\n",
    "y_p = worm_pitch_diameter / 2\n",
    "r_w = teeth * module / 2\n",
    "clearence = 0.25\n",
    "head = 0.\n",
    "    \n",
    "# create two surfaces one for positive alpha and one for negative alpha\n",
    "for alpha_i in [-alpha, alpha]:   \n",
    "    curves = []\n",
    "    xyz = compute_involute(\n",
    "        module=module,\n",
    "        teeth=teeth, \n",
    "        height=height,\n",
    "        worm_pitch_diameter=worm_pitch_diameter,\n",
    "        num_threads=num_threads,\n",
    "        alpha=alpha_i)\n",
    "    \n",
    "    for line in xyz.transpose(1, 0, 2):\n",
    "        bs = part.BSplineCurve()\n",
    "        points = [app.Vector(*point) for point in line]\n",
    "        bs.interpolate(points)\n",
    "        curves.append(bs.toShape())\n",
    "    part.show(part.makeLoft(curves))\n",
    "\n",
    "# create cutting surfaces for head and bottom\n",
    "r_head = y_p - module * (1 + head)\n",
    "r_foot = y_p + module * (1 + clearence)\n",
    "\n",
    "phi_head = np.arcsin(height / 2 / r_head)  # from + phi to - phi\n",
    "phi_foot = np.arcsin(height / 2 / r_foot)\n",
    "\n",
    "line_head = []\n",
    "for phi_i in np.linspace(-phi_head, phi_head, 10):\n",
    "    x = r_w + y_p - np.cos(phi_i) * r_head\n",
    "    z = np.sin(phi_i) * r_head\n",
    "    line_head.append([x, 0., z, 1])\n",
    "\n",
    "line_foot = []\n",
    "for phi_i in np.linspace(-phi_head, phi_head, 10):\n",
    "    x = r_w + y_p - np.cos(phi_i) * r_foot\n",
    "    z = np.sin(phi_i) * r_foot\n",
    "    line_foot.append([x, 0., z, 1])\n",
    "\n",
    "curves_head = []\n",
    "curves_foot = []\n",
    "phi_j_max = 2 * np.pi / teeth\n",
    "for phi_j in np.linspace(-phi_j_max, phi_j_max, 10):\n",
    "    T = numeric_transformation(phi_j, np.array([0., 0., 1.]))\n",
    "    temp_points_foot = [app.Vector(*(T @ point)[:3]) for point in line_foot]\n",
    "    temp_points_head = [app.Vector(*(T @ point)[:3]) for point in line_head]\n",
    "    bsp_foot = part.BSplineCurve()\n",
    "    bsp_head = part.BSplineCurve()\n",
    "    bsp_foot.interpolate(temp_points_foot)\n",
    "    bsp_head.interpolate(temp_points_head)\n",
    "    curves_foot.append(bsp_foot.toShape())\n",
    "    curves_head.append(bsp_head.toShape())\n",
    "\n",
    "part.show(part.makeLoft(curves_foot))\n",
    "part.show(part.makeLoft(curves_head))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "id": "73769524-0356-4364-acbc-c4a28d26fc57",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "t_start_1: -2.7474774194546225\n",
      "t_start_0: 2.7474774194546225\n",
      "t0: 0.7015390936864874, x0: 3.1224989948933315, min: 1.8775010319993481\n",
      "t1: 0.7015390936864874, x1: 5.454356051426789, min: 0.47093369124192\n",
      "t0: 0.5482381702072173, x0: 3.7996710369252895, min: 1.2005967588403554\n",
      "t1: 0.5482381702072173, x1: 5.868347291705316, min: 0.8705739256491289\n",
      "t0: -2.7474774194546225, x0: 4.0, min: 3.2681962153219666\n",
      "t1: -2.7474774194546225, x1: 6.0, min: 3.2681962153219666\n",
      "t0: -2.7218186874199746, x0: 3.7996710378205463, min: 3.1634473327759642\n",
      "t1: -2.7218186874199746, x1: 5.868347293420843, min: 0.86835219843846\n",
      "t0: 0.024534606456801573, x0: 3.1224989985781515, min: 1.877501936426569\n",
      "t1: 0.024534606456801573, x1: 5.454356049869874, min: 0.45435622754073013\n"
     ]
    }
   ],
   "source": [
    "import scipy as sp\n",
    "import numpy as np\n",
    "from freecad import part\n",
    "from freecad import app\n",
    "from pygears.transformation import numeric_transformation\n",
    "\n",
    "debug = False\n",
    "def compute_involute(module=1, teeth=15, height=5, worm_pitch_diameter=10, num_threads=1, alpha=np.deg2rad(20)):\n",
    "    y = 0.\n",
    "    xw = worm_pitch_diameter / 2\n",
    "    \n",
    "    def length(x, y):\n",
    "        return (x**2 + y**2)**(0.5)\n",
    "    \n",
    "    def z(x, y, t):\n",
    "        r = length(x, y)\n",
    "        return module * num_threads * np.arcsin(y / r) / 2 + (r - xw) * np.tan(alpha) + t\n",
    "\n",
    "    def distance_pw(x, y, t):\n",
    "        return np.sqrt((xw - x) ** 2 + z(x, y, t) ** 2)\n",
    "\n",
    "    def min_root(x, y, t):\n",
    "        r0 = xw -  module # * (1 + clearence)\n",
    "        x_t = sp.optimize.minimize(lambda x: distance_pw(x, y, t), (t)).x[0]\n",
    "        return distance_pw(x, y, t) + np.abs(r0**2 - x**2 - y**2)\n",
    "\n",
    "    def min_head(x, y, t):\n",
    "        r1 = xw +  module # * (1 + clearence)\n",
    "        return distance_pw(x, y, t) + np.abs(r1**2 - x**2 - y**2)\n",
    "        \n",
    "    xyz = []        \n",
    "    r0 = xw -  module # * (1 + clearence)\n",
    "    r1 = xw +  module # * (1 + clearence)\n",
    "    t_start_0 = (r0 - xw) / np.tan(alpha)\n",
    "    t_start_1 = (r1 - xw) / np.tan(alpha)\n",
    "\n",
    "    \n",
    "    for y in np.linspace(-height / 2, height / 2, 20):\n",
    "        for t in np.linspace(t_start_0, t_start_1, 20):\n",
    "            x_t = sp.optimize.minimize(lambda x: distance_pw(x, y, t), xw).x[0]\n",
    "            z_t = z(x_t, y, t)\n",
    "            point = App.Vector(x_t, y, z_t)\n",
    "            part.show(part.Point(point).toShape())\n",
    "    \n",
    "    \n",
    "compute_involute()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "26079daf-9e99-4365-89f4-c560e2aac9bd",
   "metadata": {},
   "outputs": [
    {
     "ename": "SyntaxError",
     "evalue": "expected ':' (2870794076.py, line 1)",
     "output_type": "error",
     "traceback": [
      "\u001b[0;36m  Cell \u001b[0;32mIn[20], line 1\u001b[0;36m\u001b[0m\n\u001b[0;31m    def debug_surface()\u001b[0m\n\u001b[0m                       ^\u001b[0m\n\u001b[0;31mSyntaxError\u001b[0m\u001b[0;31m:\u001b[0m expected ':'\n"
     ]
    }
   ],
   "source": [
    "    def debug_surface()\n",
    "        x_i = np.linspace(r_w / 10., r_w * 3. / 2., 10)\n",
    "        y_i = np.linspace(-r_w, r_w, 10)\n",
    "    \n",
    "    \n",
    "        # Gitterpunkte berechnen\n",
    "        xv, yv = np.meshgrid(x_i, y_i)\n",
    "        zv = z(xv, yv)\n",
    "        \n",
    "        # Punkte für die B-Spline-Fläche erzeugen\n",
    "        points = []\n",
    "        for i in range(xv.shape[0]):\n",
    "            row = []\n",
    "            for j in range(xv.shape[1]):\n",
    "                point = FreeCAD.Vector(xv[i, j], yv[i, j], zv[i, j])\n",
    "                # part.show(part.Point(point).toShape())\n",
    "                row.append(point)\n",
    "            points.append(row)\n",
    "        \n",
    "        # B-Spline-Fläche erstellen\n",
    "        bspline_surface = Part.BSplineSurface()\n",
    "        bspline_surface.approximate(points)\n",
    "        \n",
    "        # Fläche in FreeCAD-Dokument einfügen\n",
    "        doc = FreeCAD.ActiveDocument if FreeCAD.ActiveDocument else FreeCAD.newDocument()\n",
    "        bspline_shape = Part.Shape(bspline_surface)\n",
    "        part_object = doc.addObject(\"Part::Feature\", \"BSplineSurface\")\n",
    "        part_object.Shape = bspline_shape\n",
    "        doc.recompute()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "e0bfeffb-7261-4976-8eb3-805af31d4bfd",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.13.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
